/**
* Real-time SLAM with webcam/camera input (Headless version)
* Based on ORB-SLAM2 and CubeSLAM - No GUI required
*/

#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include <iomanip>
#include <opencv2/core/core.hpp>
#include <opencv2/videoio.hpp>
#include "Parameters.h"
#include "System.h"

using namespace std;

int main(int argc, char **argv)
{
    if(argc < 3 || argc > 4)
    {
        cerr << endl << "Usage: ./mono_realtime_headless path_to_vocabulary path_to_settings [camera_id]" << endl;
        cerr << "Example: ./mono_realtime_headless ./Vocabulary/ORBvoc.txt Examples/config/Realtime.yaml 0" << endl;
        return 1;
    }

    // Camera ID (default 0 for webcam)
    int camera_id = 0;
    if(argc == 4)
    {
        camera_id = atoi(argv[3]);
    }

    // Create SLAM system (disable viewer for headless mode)
    ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::MONOCULAR, false);

    // Open camera
    cv::VideoCapture cap(camera_id);
    if(!cap.isOpened())
    {
        cerr << "Error: Cannot open camera " << camera_id << endl;
        return 1;
    }

    // Set camera properties
    cap.set(cv::CAP_PROP_FRAME_WIDTH, 640);
    cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480);
    cap.set(cv::CAP_PROP_FPS, 30);

    cout << endl << "-------" << endl;
    cout << "Starting headless real-time SLAM..." << endl;
    cout << "Camera ID: " << camera_id << endl;
    cout << "Press Ctrl+C to quit" << endl << endl;

    cv::Mat im;
    int frame_count = 0;
    auto start_time = std::chrono::steady_clock::now();
    auto last_status_time = start_time;

    while(true)
    {
        // Capture frame
        cap >> im;
        if(im.empty())
        {
            cerr << "Failed to capture frame" << endl;
            break;
        }

        // Get timestamp
        auto current_time = std::chrono::steady_clock::now();
        double timestamp = std::chrono::duration_cast<std::chrono::duration<double>>(current_time - start_time).count();

        // Process frame
        cv::Mat pose = SLAM.TrackMonocular(im, timestamp, frame_count, "frame_" + to_string(frame_count));

        // Print status every 5 seconds
        if(std::chrono::duration_cast<std::chrono::seconds>(current_time - last_status_time).count() >= 5)
        {
            cout << "Frame: " << frame_count 
                 << ", Time: " << fixed << setprecision(2) << timestamp 
                 << "s, FPS: " << frame_count / timestamp;
            
            if(!pose.empty())
            {
                cout << ", Tracking: OK";
            }
            else
            {
                cout << ", Tracking: LOST";
            }
            cout << endl;
            
            last_status_time = current_time;
        }

        frame_count++;
        
        // Small delay to prevent excessive CPU usage
        usleep(1000); // 1ms delay
    }

    // Cleanup
    cap.release();
    
    // Stop all threads
    SLAM.Shutdown();

    // Save camera trajectory
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory_realtime_headless.txt");

    cout << "Headless real-time SLAM finished!" << endl;
    cout << "Total frames processed: " << frame_count << endl;
    cout << "Average FPS: " << frame_count / timestamp << endl;
    
    return 0;
}
